#include <bits/stdc++.h>
#include <fcntl.h>
#include <unistd.h>
#include <termios.h>
#include <opencv2/opencv.hpp>

using namespace std;
using namespace cv;

#define INF 10000

////////////////////////////////////////////////////////////////
// 辅助函数
// zheng数组中第一位进行比较
bool cmp1(vector<double> a, vector<double> b)
{
    return a[0] > b[0];
}

// 斜率控制中，寻找底线交点
int myget_dots(vector<double> N)
{
    double k = N[0];
    double b = N[2] - k * N[1];
    return (int)((240 - b) / k);
}

// 串口配置
void serial_config(termios &options, int fd)
{
    tcgetattr(fd, &options);
    cfsetispeed(&options, B115200); // 波特率115200
    cfsetospeed(&options, B115200);
    options.c_cflag |= (CLOCAL | CREAD); // 启用接收器和本地模式
    options.c_cflag &= ~PARENB;          // 禁用奇偶校验
    options.c_cflag &= ~CSTOPB;          // 停止位为1
    options.c_cflag &= ~CSIZE;           // 清除数据位设置
    options.c_cflag |= CS8;              // 8位数据位
    options.c_cflag &= ~CRTSCTS;         // 禁用硬控流

    tcsetattr(fd, TCSANOW, &options); // 使能设置
}
////////////////////////////////////////////////////////////////

double houghline_blue(Mat img)
{
    // 先提取蓝色，再进行霍夫直线变换，存储在lines
    Mat img_hsv;
    Mat mask_gray;
    cvtColor(img, img_hsv, COLOR_BGR2HSV);
    inRange(img_hsv, Scalar(116, 20, 90), Scalar(179, 255, 255), mask_gray);
    bitwise_and(img, img, img, mask_gray);
    cvtColor(img, mask_gray, COLOR_BGR2GRAY);
    Mat img_edges;
    Canny(mask_gray, img_edges, 200, 255);
    vector<Vec4f> lines;
    HoughLinesP(img_edges, lines, 1, (double)3.1415926535 / 180, 40, 50, 200);

    // lines为空的特殊处理
    if (lines.size() == 0)
    {
        cout << "no blue line" << endl;
        return -INF;
    }

    // 求K的中位数
    vector<double> k_arr;
    for (auto &line : lines)
    {
        double k = (line[1] - line[3]) / (line[0] - line[2]);
        if (abs(k) < 0.2)
        {
            k_arr.push_back(k);
            cv::line(img, Point(line[0], line[1]), Point(line[2], line[3]), Scalar(0, 255, 0));
        }
    }

    if (k_arr.size() == 0)
    {
        cout << "no k in range" << endl;
        return -INF;
    }

    sort(k_arr.begin(), k_arr.end());
    double k = k_arr[k_arr.size() / 2];
    return k;
}

pair<int, int> houghline_k(Mat img)
{
    pair<int, int> p;
    // 霍夫直线变换，存储在lines
    Mat gray, binary;
    cvtColor(img, gray, COLOR_BGR2GRAY);
    threshold(gray, binary, 150, 255, THRESH_TOZERO);
    Mat edges;
    Canny(gray, edges, 200, 255);
    vector<Vec4f> lines;
    HoughLinesP(edges, lines, 1, 3.1415926535 / 180, 50, 50, 20);

    // 在斜率范围内的直线进行记录
    vector<vector<double>> zheng;
    vector<vector<double>> fu;
    if (lines.size() == 0)
    {
        cout << "no lines in left or right" << endl;
        return make_pair(-INF, INF);
    }
    for (auto &line : lines)
    {
        vector<double> temp = {0, 0, 0, 0, 0};
        if (line[0] == line[2])
            continue;
        double k = (line[1] - line[3]) / (line[0] - line[2]);
        if (k > 0.3 && k < 1.3)
        {
            temp[0] = k;
            temp[1] = line[0];
            temp[2] = line[1];
            temp[3] = line[2];
            temp[4] = line[3];
            zheng.push_back(temp);
        }
        else if (k < -0.3 && k > -0.8)
        {
            temp[0] = k;
            temp[1] = line[0];
            temp[2] = line[1];
            temp[3] = line[2];
            temp[4] = line[3];
            fu.push_back(temp);
        }
    }

    // 右侧方向仅存储斜率最大的
    // 左侧进行求平均值处理
    if (zheng.size())
    {
        sort(zheng.begin(), zheng.end(), cmp1);
        p.second = myget_dots(zheng[0]);
    }
    else
        p.second = INF;

    if (fu.size())
    {
        vector<double> a = {0, 0, 0, 0, 0};
        for (auto &i : fu)
            for (int j = 0; j < 5; j++)
                a[j] += i[j];
        for (int j = 0; j < 5; j++)
            a[j] /= fu.size();

        p.first = myget_dots(a);
    }
    else
        p.first = -INF;

    return p;
}

// 蓝线策略
// ±lanxian1以内为直行，1~2为1档,2以上为2档
#define lanxian1 0.05
#define lanxian2 0.1
char strategy_blue(double s)
{
    // 蓝线优先级最高
    if (s != -INF)
    {
        if (s < -lanxian2)
            return '1';
        if (s < -lanxian1)
            return '2';
        if (s < lanxian1)
            return '3';
        if (s < lanxian2)
            return '4';
        return '5';
    }
    return '0';
}

// 斜率策略
// 参数已经基本上调好，不进行宏定义设置
char strategy_k(int left, int right)
{
    int m = (left + right) / 2;
    int temp = 310;
    if (left == -INF && right == INF)
        return '0';
    if (left == -INF)
    {
        m = right;
        temp = 670;
    }
    if (right == INF)
    {
        m = left;
        temp = -50;
    }
    m = m - temp;

    if (m < -50)
        return '1';
    else if (m < -20)
        return '2';
    else if (m < 75)
        return '3';
    else if (m < 130)
        return '4';
    return '5';
}

#define USART 0

int main()
{
#if USART
    // 打开串口
    char sendbuff[10] = "";
    char resbuff[10] = "";
    const char *serialPort = "/dev/ttyUSB0";
    int fd = open(serialPort, O_RDWR | O_NOCTTY | O_NDELAY);
    if (fd == -1)
    {
        cout << "Failed to open serial port!" << endl;
    }
    // 串口配置
    struct termios options;
    tcgetattr(fd, &options);
#endif

    // 打开摄像头
    VideoCapture camera = VideoCapture(0);
    Mat img;
    char sign = '3';
    while (1)
    {
        camera.read(img);
        imshow("img", img);

        // 蓝线策略
        double k = houghline_blue(img);
        sign = strategy_blue(k);
        if (sign != '0')
        {
#if USART
            sendbuff[0] = sign;
            sendbuff[1] = '\r';
            sendbuff[2] = '\n';
            sendbuff[3] = '\0';
            write(fd, sendbuff, strlen(sendbuff));
#else
            cout << sign << endl;
#endif
        }
        else
        {
            // 斜率策略
            pair<int, int> lr = houghline_k(img);
            sign = strategy_k(lr.first, lr.second);
            if (sign != '0')
            {
#if USART
                sendbuff[0] = sign;
                sendbuff[1] = '\r';
                sendbuff[2] = '\n';
                sendbuff[3] = '\0';
                write(fd, sendbuff, strlen(sendbuff));
#else
                cout << sign << endl;
#endif
            }
            else
            {
                cout << "轨迹预测" << endl;
                // 轨迹预测策略/////////////////////////////////////////////////////////
                // 轨迹预测策略/////////////////////////////////////////////////////////
                // 轨迹预测策略/////////////////////////////////////////////////////////
                // 轨迹预测策略/////////////////////////////////////////////////////////
                // 轨迹预测策略/////////////////////////////////////////////////////////
            }
        }

        char key = waitKey(100);
        if (key == 'q')
        {
            break;
        }
    }
#if USART
    close(fd);
#endif
    destroyAllWindows();

    return 0;
}